#include "imu.h"
#include "unistd.h"
#include <stdio.h>

#include "rpi.h"
#include "motorcontroller.h"

#define TARGET_FREQ 50

int loops;

int main(int argc, char * argv[])
{
  RPi::initialize();



  IMU imu; 
  MotorController mc(RPi::i2cFile());
  
  if (argc > 1)
  {
    mc.setVoltage(-1, 0);
    usleep(1000);
    return 0;
  }

  printf("done\n");

  imu.setup();

  imu.initialize();

  // 
  usleep(IMU_MIN_SENSOR_REST * 1000);

  int loops = 0;

  int target_time_diff = 1.0 / TARGET_FREQ * 1000 * 1000;

  long time_last = RPi::usec();
  double avg_freq = 50.0;
  double avg_freq_weight = 0.1;


  double pwm_mean = 0.3;
  double pwm_margin = 0.2;

  double pwm_b_pitch = 0;
  double pwm_b_roll = 0;

  double pwm_t_pitch = 0;
  double pwm_t_roll = 0;

  double err_pitch = 0, err_pitch_last = 0;
  double err_roll = 0, err_roll_last = 0;

  double avg_pitch = 0;
  double avg_roll = 0;

  double avg_fact = 0.6;


  while (true)
  {
    int time_diff = RPi::usec() - time_last;
    //printf("time diff: %d\n", time_diff);
    if (time_diff < target_time_diff)
    {
      usleep((target_time_diff - time_diff)/1.45);
    }

    long time1 = RPi::usec();
    imu.read(); // subsamples - time unknown, approx enough to get 50Hz
    long time2 = RPi::usec();
    
    // **
    time_diff = RPi::usec() - time_last;
    avg_freq = (avg_freq_weight / time_diff * 1000 * 1000) + (1-avg_freq_weight)*avg_freq;
    // **

    imu.process(); // 4 milisec

    Vector3D m, a, g;

    m = imu.getM();
    a = imu.getA();
    g = imu.getG();

    printf("d %6.3fms %.0fHz    %9.6f %9.6f %9.6f      %9.6f %9.6f %9.6f       %9.6f %9.6f %9.6f   %9.6f\n", 
      //millis(),
      imu.getT()/1000,
      //time_diff,
      //time2-time1,
      avg_freq,
      a.x(), a.y(), a.z(), 
      m.x(), m.y(), m.z(), 
      g.x(), g.y(), g.z(),
      m.x()*m.x()+m.y()*m.y()+m.z()*m.z()
      );

    time_last = RPi::usec();

    //continue;
    Quaternion pos;

    double pitch, roll;
    //pitch = asin(qarot.x());
    //roll = atan2(-qarot.y(),qarot.z());
    pitch = 0;
    roll = 0;

    



/*
    for (int i = 0; i < 4; i++)
      printf("%6.3f ", qarot(i));
    for (int i = 0; i < 4; i++)
      printf("%6.3f ", imu.qavga(i));
    for (int i = 0; i < 4; i++)
      printf("%6.3f ", qa(i));
    printf("\n");

    pos = imu.imup.getPosRawAcc();
    for (int i = 0; i < 4; i++)
      printf("%6.3f ", pos(i));
    printf("\n");
    */

/*
  Quaternion ainp(0, accinput.x(), accinput.y(), accinput.z());
  Quaternion aref(0, avga.x(), avga.y(), avga.z());
  Quaternion arot = aref.conjugate() * ainp * aref;

  
  printf("simple: \n");
  printvector(ainp);
  printvector(aref);
  printvector(arot);
  printf("\n");

*/

    VectorND<7> kal = imu.imup.getPosKalman();
    printf("K ");
    for (int i = 0; i < 7; i++)
    {
      printf("%10.4f ", kal(i));
    }
    printf("\n");

    /*
    printf("A %9.6f %9.6f %9.6f %9.6f ",
      pos.w(), pos.x(), pos.y(), pos.z()
      );
    pos = imu.imup.getPosRawAccError();
    printf("%9.6f %9.6f %9.6f %9.6f ",
      pos.w(), pos.x(), pos.y(), pos.z()
      );
    printf("\n");
    */

    /*
    pos = imu.imup.getPosRawGyr();
    printf("G %9.6f %9.6f %9.6f %9.6f ",
      pos.w(), pos.x(), pos.y(), pos.z()
      );
    pos = imu.imup.getPosRawGyrError();
    printf("%9.6f %9.6f %9.6f %9.6f ",
      pos.w(), pos.x(), pos.y(), pos.z()
      );
    printf("\n");
    */

    pos = imu.imup.getPosNewton();

    double yaw;

/*
    roll = 2 * (pos(0)*pos(1) - pos(2)*pos(3)) / (pos(0)*pos(0) - pos(1)*pos(1) - pos(2)*pos(2) + pos(3)*pos(3) );
    roll = atan(roll);
    */
    roll = roll / 3.14 * 180;

/*
    pitch = 2 * (pos(0)*pos(2) + pos(3)*pos(1));
    pitch = asin(pitch);
    */
    pitch = pitch / 3.14 * 180;

    yaw = 0;
    /*
    yaw = 2 * (pos(0)*pos(3) - pos(1)*pos(2)) / (pos(0)*pos(0) + pos(1)*pos(1) - pos(2)*pos(2) - pos(3)*pos(3) );
    yaw = atan(roll);
    */
    yaw = yaw / 3.14 * 180;

    avg_roll = avg_fact * avg_roll + (1 - avg_fact) * roll;
    avg_pitch= avg_fact * avg_pitch+ (1 - avg_fact) * pitch;


    //printf("rpy: %9.3f %9.3f %9.3f \n", pitch, roll, yaw);

    // (************************************************


    double pconst = 0.05 * 0;
    double dconst = 0.1 * 0;
    
    err_pitch = pwm_t_pitch - avg_pitch;
    err_roll = pwm_t_roll - avg_roll;

    double derr_pitch = err_pitch - err_pitch_last;
    double derr_roll  = err_roll  - err_roll_last;

    err_pitch_last = err_pitch;
    err_roll_last =  err_roll;
    
    pwm_b_pitch = pconst * err_pitch + dconst * derr_pitch; 
    pwm_b_roll  = pconst * err_roll  + dconst * derr_roll; 

    
    double mstrength[4];
    for (int i = 0; i < 4; i++)
      mstrength[i] = pwm_mean;

    mstrength[0] -= pwm_margin * atan(pwm_b_pitch) / 3.14159; 
    mstrength[1] -= pwm_margin * atan(pwm_b_pitch) / 3.14159; 
    mstrength[2] += pwm_margin * atan(pwm_b_pitch) / 3.14159; 
    mstrength[3] += pwm_margin * atan(pwm_b_pitch) / 3.14159; 

    mstrength[0] -= pwm_margin * atan(pwm_b_roll) / 3.14159; 
    mstrength[3] -= pwm_margin * atan(pwm_b_roll) / 3.14159; 
    mstrength[1] += pwm_margin * atan(pwm_b_roll) / 3.14159; 
    mstrength[2] += pwm_margin * atan(pwm_b_roll) / 3.14159; 

   
   
    if (false)
    {
    for (int i = 0; i < 4; i++)
      printf("%6.2f ", mstrength[i]);
    printf("\n");
    }

    for (int i = 0; i < 4; i++)
    {
      //mc.setVoltage(i, mstrength[i]);
    }

    /*
    printf("K ");
    printf("%9.6f %9.6f %9.6f %9.6f   %9.6f %9.6f %9.6f\n",
      kal(0), kal(1), kal(2), kal(3), kal(4), kal(5), kal(6)
      );
    */
/*
    kal = imu.getPosKalmanErr();
    printf("K ");
    printf("%9.6f %9.6f %9.6f %9.6f   %9.6f %9.6f %9.6f\n",
      kal(0), kal(1), kal(2), kal(3), kal(4), kal(5), kal(6)
      );
    */


    loops++;
  }


}

